clear all ; close all


data = load('newquad_hacker.txt');

tout = size(data,1);
t = [1:1:tout]*0.025;

pos = data(:,1:3);
posd = data (:,7:9);

att = data(:,4:6);
attd = data(:,10:12);

vel = data(:, 13:18);

U = data(:,19:22);

%% position

fg1 = figure(1)
set(fg1, 'color', [1 1 1])
subplot(3,1,1)
plot(t,pos(:,1),'color',[0 0 0], 'linewidth', 2, 'linestyle','-')
hold on
plot(t,posd(:,1),'color',[0 0 0], 'linewidth', 1.2, 'linestyle','--')
ylabel('x (m)')

subplot(3,1,2)
plot(t,pos(:,2),'color',[0 0 0], 'linewidth', 2, 'linestyle','-')
hold on
plot(t,posd(:,2),'color',[0 0 0], 'linewidth', 1.2, 'linestyle','--')
ylabel('y (m)')

subplot(3,1,3)
plot(t,pos(:,3),'color',[0 0 0], 'linewidth', 2, 'linestyle','-')
hold on
plot(t,posd(:,3),'color',[0 0 0], 'linewidth', 1.2, 'linestyle','--')
ylabel('z (m)')

%% attitude

fg2 = figure(2)
set(fg2, 'color', [1 1 1])
subplot(3,1,1)
plot(t,-att(:,1)*180/pi,'color',[0 0 0], 'linewidth', 2, 'linestyle','-')
hold on
plot(t,attd(:,1)*180/pi,'color',[0 0 0], 'linewidth', 1.2, 'linestyle','--')
ylabel('phi (deg)')

subplot(3,1,2)
plot(t,att(:,2)*180/pi,'color',[0 0 0], 'linewidth', 2, 'linestyle','-')
hold on
plot(t,attd(:,2)*180/pi,'color',[0 0 0], 'linewidth', 1.2, 'linestyle','--')
ylabel('theta (deg)')

subplot(3,1,3)
plot(t,att(:,3)*180/pi,'color',[0 0 0], 'linewidth', 2, 'linestyle','-')
hold on
plot(t,attd(:,3)*180/pi,'color',[0 0 0], 'linewidth', 1.2, 'linestyle','--')
ylabel('psi (deg)')

%% input

fg3 = figure(3)
set(fg2, 'color', [1 1 1])
subplot(4,1,1)
plot(t,U(:,1),'color',[0 0 0], 'linewidth', 1.5, 'linestyle','-')

ylabel('u1')

subplot(4,1,2)
plot(t,U(:,2),'color',[0 0 0], 'linewidth', 1.5, 'linestyle','-')
ylabel('u2')

subplot(4,1,3)
plot(t,U(:,3),'color',[0 0 0], 'linewidth', 1.5, 'linestyle','-')
hold on
ylabel('u3')

subplot(4,1,4)
plot(t,U(:,3),'color',[0 0 0], 'linewidth', 1.5, 'linestyle','-')
ylabel('u4')






